#include <TinyGPSPlus.h>
#include <SoftwareSerial.h>
#include "CalculateSunRise.h"
#include "CalculateSunAngle.h"

#define North 'N'
#define South 'S'
#define Centre 'C'

// 转向速度  太阳传感器工作时，如果与实际太阳角度差别较大，先用较快速度旋转，等差别小时用慢速度旋转，防止较快的速度过度转向
int TurningSpeedFaster = 1;
int TurningSpeedSlow = 2;
int TurningSpeed0 = 0 ;// 停止状态
// 太阳传感器光斑居中时的值域
double minPxPy = 49.9;
double maxPxPy = 50.1;

// 当前时间
struct tm *now;


#define rs485Pin 8

SoftwareSerial rs485Serial(52,9);

TinyGPSPlus gps;
CalculateSunRise csr;
CalculateSunAngle csa;

// 太阳传感器工作模式时，不计算太阳角度与云台角度，直接由太阳传感器控制云台转动
bool sunSensorMode = false;
// 连续该次数未发现太阳时，则变为非太阳传感器工作模式
int moreThanCount2NotSunSensorMode = 3;
// 太阳传感器工作模式时，未发现太阳的次数：太阳传感器被遮挡时计数一次。
int notFoundSunCount = 0;

// 南回归线以南south、北回归线以北north、南北回归线之间centre
char location = North;

void setup(){
  // 初始化时等待10秒：防止程序无法启动。
  delay(10 * 1000);
  rs485Serial.begin(9600);
  Serial.begin(9600);
  Serial1.begin(115200); // sun sensor
  Serial2.begin(9600); // gps
}

void loop(){
  while(Serial2.available() > 0){
    gps.encode(Serial2.read());
    if(gps.location.isUpdated() == true){
      int zone = caculateTimeZone(gps.location.lng());
      struct tm t;
      t.tm_year = gps.date.year() - 1900;
      t.tm_mon = gps.date.month() - 1;
      t.tm_mday = gps.date.day();
      t.tm_hour = gps.time.hour();
      t.tm_min = gps.time.minute();
      t.tm_sec = gps.time.second();
      time_t time =  mktime(&t);
      time = time + zone * 60 * 60;
      now = localtime(&time);
      if(gps.location.lat() > 0){
        location = North;
      }else if(gps.location.lat() < 0){
        location = South;
      }
      // c = gmtime(&time);
      bool sunIsVisible = csr.bDaytime(gps.location.lng(), gps.location.lat(),  now->tm_year + 1900, now->tm_mon + 1, now->tm_mday, now->tm_hour, now->tm_min);
      if(sunIsVisible == true){
        struct SunAngleStruct sunAngle = csa.caculate(now->tm_year + 1900, now->tm_mon + 1, now->tm_mday, now->tm_hour, now->tm_min, now->tm_sec, gps.location.lng(), gps.location.lat());
        Serial.println("Alt: " + (String)sunAngle.alt);
        Serial.println("Azi: " + (String)sunAngle.azi);
        Serial.println(ctime(&time));
        // 如果太阳角度超出了云台的工作范围，那么停止工作
        if(sunAngle.azi > 270 || sunAngle.azi < 90 || sunAngle.alt > 90 || sunAngle.alt < 0){
          Serial.println("The sun angle is beyond the range");
          delay(20000);
          return;
        }
        controlDevice(sunAngle.azi,sunAngle.alt);
      }else{
        Serial.println("sun is not visible");
        delay(20000);
      }
    }
  }
}

// 计算时区
int caculateTimeZone(double longitude){
  if (abs((int)longitude % 0xf) <= 7.5) {
      return (int)(longitude / 0xf);
  } else {
      return (int)(longitude / 0xf) + (longitude > 0x0 ? 0x1 : -0x1);
  }
}

// 设置485为读模式
void setRs485ReadMode(){
  pinMode(rs485Pin,OUTPUT);
  digitalWrite(rs485Pin,LOW);
}
// 设置485为写模式
void setRs485WriteMode(){
  pinMode(rs485Pin,INPUT);
  digitalWrite(rs485Pin,HIGH);
}
// 获取云台水平角度
char cmd_deviceHAngle[7] = {0xFF, 0x01, 0x00, 0x51, 0x00, 0x00, 0x52};
// 获取云台俯仰角度
char cmd_deviceVAngle[7] = {0xFF, 0x01, 0x00, 0x53, 0x00, 0x00, 0x54};
// 水平方向停止移动
char cmd_deviceHMoveStop[7] = {0xFF, 0x01, 0x00, 0x02, 0x00, 0x00, 0x03};
// 云台慢速向左转动
char cmd_deviceLeftMoveLow[7] = {0xFF, 0x01, 0x00, 0x04, 0x05, 0x00, 0x0A};
char cmd_deviceLeftMoveLow2[7] = {0xFF, 0x01, 0x00, 0x04, 0x01, 0x00, 0x06};

// 云台慢速向右转动
char cmd_deviceRightMoveLow[7] = {0xFF, 0x01, 0x00, 0x02, 0x05, 0x00, 0x08};
char cmd_deviceRightMoveLow2[7] = {0xFF, 0x01, 0x00, 0x02, 0x01, 0x00, 0x04};

// 云台慢速向下转动
char cmd_deviceDownMoveLow[7] = {0xFF, 0x01, 0x00, 0x10, 0x00, 0x05, 0x16};
char cmd_deviceDownMoveLow2[7] = {0xFF, 0x01, 0x00, 0x10, 0x00, 0x01, 0x12};

// 云台慢速向上转动
char cmd_deviceUpMoveLow[7] = {0xFF, 0x01, 0x00, 0x08, 0x00, 0x05, 0x0E};
char cmd_deviceUpMoveLow2[7] = {0xFF, 0x01, 0x00, 0x08, 0x00, 0x01, 0x0A};
// 垂直方向停止移动
char cmd_deviceVMoveStop[7] = {0xFF, 0x01, 0x00, 0x08, 0x00, 0x00, 0x09};

// 云台与太阳角度的换算角度 例如：云台竖直向上为0度，对应的太阳俯仰角为90度，则该角度为90度
double deviceVAngle2SunAngle = 90;
int hexArray[4];
// 获取云台角度 direction: v|h
double getDeviceAngle(char direction){
  int i = 0;
  byte res[7] = {};
  setRs485WriteMode();
  if(direction == 'h'){
    rs485Serial.write(cmd_deviceHAngle,7);
  }else{
    rs485Serial.write(cmd_deviceVAngle,7);
  }
  setRs485ReadMode();
  while(rs485Serial.available()){
    res[i] = rs485Serial.read();
    i++;
  }
  if(direction == 'h'){
    return (double)((long)res[4] << 8 | (long)res[5]) / 100;
  }
  if((double)((long)res[4] << 8 | (long)res[5]) / 100 > 180){
    return 360 - (double)((long)res[4] << 8 | (long)res[5]) / 100;
  }else{
    return -(double)((long)res[4] << 8 | (long)res[5]) / 100;
  }
}

char _16[] = {0x0,0x1,0x2,0x3,0x4,0x5,0x6,0x7,0x8,0x9,0xA,0xB,0xC,0xD,0xE,0xF};
// 十进制转16进制
void toHex(int n){
  hexArray[0] = 0;
  hexArray[1] = 0;
  hexArray[2] = 0;
  hexArray[3] = 0;
  if(n == 0){
    return hexArray;
  }
  
  int j = 4;
  int i;
  while(n){
    j--;
    i = n % 16;
    hexArray[j] = _16[i];
    n /= 16;
  }
}
// 控制云台
void controlDevice(double sunHAngle, double sunVAngle){
  // 非太阳传感器工作模式
  if(sunSensorMode == false){
    getDeviceAngle('h');
    delay(1000);
    double angleDiff = 0;
    // 慧安云台 double deviceHAngle = 360 - getDeviceAngle('h'); 
    // 慧安修改值域后的云台 double deviceHAngle = getDeviceAngle('h'); 
    // 中安视通云台 double deviceHAngle = getDeviceAngle('h');
    // todo 不同云台设置不同
    double deviceHAngle = getDeviceAngle('h');
    // 南半球
    if(location == South){
      // 东-北（90-0 度）：设备水平角度-90 为太阳角度
      if(sunHAngle >= 0 && sunHAngle <= 90){
        deviceHAngle = deviceHAngle - 90;
        // 北-西（360-270 度）：设备水平角度+270 为太阳角度
      }else if(sunHAngle < 360 && sunHAngle >= 270){
        deviceHAngle = deviceHAngle + 270;
      }
    }else{ // 北半球，设备水平角度+90 为太阳角度
      deviceHAngle = deviceHAngle + 90;
    }
    delay(1000);
    getDeviceAngle('v');
    delay(1000);
    double deviceVAngle = getDeviceAngle('v') + deviceVAngle2SunAngle;
    delay(1000);
    setRs485WriteMode();
    Serial.println("deviceHAngle" + (String)deviceHAngle);
    Serial.println("deviceVAngle" + (String)deviceVAngle);
    // 判断垂直角度是否差1度以上
    if((angleDiff = fabs(deviceVAngle - sunVAngle)) >= 1){
      Serial.println("v diff: " + (String) angleDiff);
      unsigned char v_quick_move_cmd[7] = {0xFF, 0x01, 0x08, 0x4D, 0, 0, 0};
      if(sunVAngle >= deviceVAngle2SunAngle){
        toHex(36000 - (sunVAngle - deviceVAngle2SunAngle) * 100);
      }else{
        toHex((deviceVAngle2SunAngle - sunVAngle) * 100);
      }
      v_quick_move_cmd[4] = hexArray[0] * 0x10 + hexArray[1];
      v_quick_move_cmd[5] = hexArray[2] * 0x10 + hexArray[3];
      v_quick_move_cmd[6] = 0x00 + v_quick_move_cmd[1] + v_quick_move_cmd[2] + v_quick_move_cmd[3] + v_quick_move_cmd[4] + v_quick_move_cmd[5];
      rs485Serial.write(v_quick_move_cmd,7);
      delay(angleDiff / 2.0 * 1000);
    }
    // 判断水平角度是否差1度以上
    if((angleDiff = fabs(deviceHAngle - sunHAngle)) >= 1){
      Serial.println("h diff: " + (String) angleDiff);
      char h_quick_move_cmd[7] = {0xFF, 0x01, 0x08, 0x4B, 0, 0, 0};
      // todo 不同云台设置不同
       // 南半球
       if(location == South){
          // 东-北（90-0 ）：太阳角度+90 为云台角度
          if(sunHAngle >= 0 && sunHAngle <= 90){
            toHex((90 + sunHAngle) * 100);
            // 北-西（360-270）：太阳角度-270 为设备角度
          }else if(sunHAngle < 360 && sunHAngle >= 270){
            toHex((sunHAngle - 270) * 100);
          }
       }else{ // 北半球，太阳角度-90 为云台角度
          // 原慧安云台 toHex((360 - sunHAngle) * 100);
          // 调整角度域后的慧安云台 toHex((sunHAngle - 90) * 100);
          // 中安视通云台 toHex(sunHAngle * 100);
          toHex((sunHAngle - 90) * 100);
       }
      h_quick_move_cmd[4] = hexArray[0] * 0x10 + hexArray[1];
      h_quick_move_cmd[5] = hexArray[2] * 0x10 + hexArray[3];
      h_quick_move_cmd[6] = 0x00 + h_quick_move_cmd[1] + h_quick_move_cmd[2] + h_quick_move_cmd[3] + h_quick_move_cmd[4] + h_quick_move_cmd[5];
      rs485Serial.write(h_quick_move_cmd,7);
      delay(angleDiff / 2.0 * 1000);
    }
  }
  Serial1.write("#SI:02%");
  delay(100);
  //太阳传感器光斑位置 
  double px = 0;
  double py = 0;
  int turnLeft = TurningSpeed0;
  int turnRight = TurningSpeed0;
  int turnUp = TurningSpeed0;
  int turnDown = TurningSpeed0;

  int sunSensorDataVoltage[12] = {};
  int i = 0;
  // 读取几次太阳传感器数据
  double nSunSensorData = 1.0;
  // 上下左右调整
  while((py < minPxPy || py > maxPxPy) || (px < minPxPy || px > maxPxPy)){
    // 左右调整
    while(px < minPxPy || px > maxPxPy){
      double D00VoltageSum = 0; 
      double D01VoltageSum = 0;
      double D10VoltageSum = 0;
      double D11VoltageSum = 0;
      for(int j = 0; j < nSunSensorData; j++){
        sunSensorDataVoltage[12] = {};
        // 获取太阳传感器的数据，微调云台对准太阳
        Serial1.write("#?data%");
        i = 0;
        while(Serial1.available() > 0){
          sunSensorDataVoltage[i] = Serial1.read() - '0';
          i++;
        }
        D00VoltageSum += sunSensorDataVoltage[0] * 256 + sunSensorDataVoltage[1] * 16 + sunSensorDataVoltage[2];
        D01VoltageSum += sunSensorDataVoltage[3] * 256 + sunSensorDataVoltage[4] * 16 + sunSensorDataVoltage[5];
        D10VoltageSum += sunSensorDataVoltage[6] * 256 + sunSensorDataVoltage[7] * 16 + sunSensorDataVoltage[8];
        D11VoltageSum += sunSensorDataVoltage[9] * 256 + sunSensorDataVoltage[10] * 16 + sunSensorDataVoltage[11];
        delay(100);
      }
      double D00Voltage = D00VoltageSum/nSunSensorData;
      double D01Voltage = D01VoltageSum/nSunSensorData;
      double D10Voltage = D10VoltageSum/nSunSensorData;
      double D11Voltage = D11VoltageSum/nSunSensorData;
      Serial.println("Voltage : " + (String)D00Voltage + " - " + D01Voltage + " - " + D10Voltage + " - " + D11Voltage);
      // 如果光都不满200说明没有太阳
      if(D00Voltage < 200 && D01Voltage < 200 && D10Voltage < 200 && D11Voltage < 200){
        notFoundSunCount++;
        Serial.println("not found sun count: " + (String)notFoundSunCount);
        if(notFoundSunCount > moreThanCount2NotSunSensorMode){
          sunSensorMode = false;
        }
        // 停止云台移动
        rs485Serial.write(cmd_deviceHMoveStop,7);
        delay(20 * 1000);
        return;
      }
      notFoundSunCount = 0;
      sunSensorMode = true;
      px = 100 * (D01Voltage + D11Voltage) / (D00Voltage + D01Voltage + D10Voltage + D11Voltage);
      py = 100 * (D00Voltage + D01Voltage) / (D00Voltage + D01Voltage + D10Voltage + D11Voltage);
      Serial.println("px -----  py: " + (String)px + " ----- " + py);
      // 传感器的左右，与面向太阳时的左右相反。以下左右说面向太阳的左右。 光斑偏右，向左旋转
      if(px < minPxPy){
        if(px < 48 && turnLeft != TurningSpeedFaster){
          turnLeft = TurningSpeedFaster;
          rs485Serial.write(cmd_deviceLeftMoveLow,7);
        }else if(px >= 48 && turnLeft != TurningSpeedSlow){
          turnLeft = TurningSpeedSlow;
          rs485Serial.write(cmd_deviceLeftMoveLow2,7);
        }
        turnRight = TurningSpeed0;
        delay(100);
      }else if(px > maxPxPy){ // 光斑偏左，向右旋转
        if(px > 52 && turnRight != TurningSpeedFaster){
          turnRight = TurningSpeedFaster;
          rs485Serial.write(cmd_deviceRightMoveLow,7);
        }else if(px <= 52 && turnRight != TurningSpeedSlow){
          turnRight = TurningSpeedSlow;
          rs485Serial.write(cmd_deviceRightMoveLow2,7);
        }
        turnLeft = TurningSpeed0;
        delay(100);
      }
    }
    rs485Serial.write(cmd_deviceHMoveStop,7);
    turnLeft = TurningSpeed0;
    turnRight = TurningSpeed0;

    // 上下调整
    while(py < minPxPy || py > maxPxPy){
      double D00VoltageSum = 0;
      double D01VoltageSum = 0;
      double D10VoltageSum = 0;
      double D11VoltageSum = 0;
      for(int j = 0; j < nSunSensorData; j++){
        sunSensorDataVoltage[12] = {};
        // 获取太阳传感器的数据，微调云台对准太阳
        Serial1.write("#?data%");
        i = 0;
        while(Serial1.available() > 0){
          sunSensorDataVoltage[i] = Serial1.read() - '0';
          i++;
        }
        D00VoltageSum += sunSensorDataVoltage[0] * 256 + sunSensorDataVoltage[1] * 16 + sunSensorDataVoltage[2];
        D01VoltageSum += sunSensorDataVoltage[3] * 256 + sunSensorDataVoltage[4] * 16 + sunSensorDataVoltage[5];
        D10VoltageSum += sunSensorDataVoltage[6] * 256 + sunSensorDataVoltage[7] * 16 + sunSensorDataVoltage[8];
        D11VoltageSum += sunSensorDataVoltage[9] * 256 + sunSensorDataVoltage[10] * 16 + sunSensorDataVoltage[11];
        delay(100);
      }
      double D00Voltage = D00VoltageSum/nSunSensorData;
      double D01Voltage = D01VoltageSum/nSunSensorData;
      double D10Voltage = D10VoltageSum/nSunSensorData;
      double D11Voltage = D11VoltageSum/nSunSensorData;
      Serial.println("Voltage : " + (String)D00Voltage + " - " + D01Voltage + " - " + D10Voltage + " - " + D11Voltage);
      // 如果光都不满200说明没有太阳
      if(D00Voltage < 200 && D01Voltage < 200 && D10Voltage < 200 && D11Voltage < 200){
        notFoundSunCount++;
        Serial.println("not found sun count: " + (String)notFoundSunCount);
        if(notFoundSunCount > moreThanCount2NotSunSensorMode){
          sunSensorMode = false;
        }
        // 停止云台移动
        rs485Serial.write(cmd_deviceHMoveStop,7);
        delay(20 * 1000);
        return;
      }
      notFoundSunCount = 0;
      sunSensorMode = true;
      px = 100 * (D01Voltage + D11Voltage) / (D00Voltage + D01Voltage + D10Voltage + D11Voltage);
      py = 100 * (D00Voltage + D01Voltage) / (D00Voltage + D01Voltage + D10Voltage + D11Voltage);
      Serial.println("px -----  py: " + (String)px + " ----- " + py);
      // 光斑偏上，向下旋转
      if(py > maxPxPy){
        if(py > 52 && turnDown != TurningSpeedFaster){
          turnDown = TurningSpeedFaster;
          rs485Serial.write(cmd_deviceDownMoveLow,7);
        }else if(py <= 52 && turnDown != TurningSpeedSlow){
          turnDown = TurningSpeedSlow;
          rs485Serial.write(cmd_deviceDownMoveLow2,7);
        }
        turnUp = TurningSpeed0;
        delay(100);
      }else if(py < minPxPy){ // 光斑偏下，想上旋转
        if(py < 48 && turnUp != TurningSpeedFaster){
          turnUp = TurningSpeedFaster;
          rs485Serial.write(cmd_deviceUpMoveLow,7);
        }else if(py >= 48 && turnUp != TurningSpeedSlow){
          turnUp = TurningSpeedSlow;
          rs485Serial.write(cmd_deviceUpMoveLow2,7);
        }
        turnDown = TurningSpeed0;
        delay(100);
      }
    }
    rs485Serial.write(cmd_deviceVMoveStop,7);
    turnUp = TurningSpeed0;
    turnDown = TurningSpeed0;
  }
  // 云台停止移动
  // rs485Serial.write(cmd_deviceVMoveStop,7);
  // delay(20 * 1000);
}
